/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
	\file dataToKML.cxx
	\brief class implemenation KML generator

									  \author Jack Elston
						 $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>

#include "String.h"

#include <tr1/unordered_map>

#include "kml/dom.h"

#include "dataToKML.h"

using kmldom::CoordinatesPtr;
using kmldom::KmlPtr;
using kmldom::KmlFactory;
using kmldom::PlacemarkPtr;
using kmldom::PointPtr;

using kmldom::StylePtr;
using kmldom::IconStylePtr;
using kmldom::IconStyleIconPtr;

using kmldom::DocumentPtr;

using kmldom::ModelPtr;
using kmldom::LocationPtr;
using kmldom::OrientationPtr;
using kmldom::ScalePtr;
using kmldom::LinkPtr;

/*<-------Defines--------->*/
  typedef std::tr1::unordered_map<const char *, PlacemarkPtr> placemark_hash_t;
/*<------End Defines------>*/

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
  extern bool verbose;

  placemark_hash_t placemarks;

  KmlFactory* factory;
  KmlPtr kml;
  DocumentPtr document;

  fstream kml_file;
/*<-End Global Variables-->*/

/************************************************************\
  |                  DataToKML_Parser::init                    |
  \************************************************************/
/*!
  \brief the initializing function for the KML generating thread.
  \param shm shared memory struct between the gateway threads.
  \pre instance of DataToKML_Parser has been created
  \post the KML utils have been intialized and the inetTxFifo
  and SAVRxFifo have been opened
  \return TRUE on successful completion of initialization, FALSE otherwise
  */
DataToKML_Parser::DataToKML_Parser(String name) : ThreadedPipe(name)
{
	factory = KmlFactory::GetFactory();
	kml = factory->CreateKml();
	document = factory->CreateDocument();
	kml->set_feature(document);

	StylePtr ua_style = factory->CreateStyle();
	ua_style->set_id("NexSTAR");
	IconStyleIconPtr icon = factory->CreateIconStyleIcon();
	icon->set_href("http://tornadochaser.colorado.edu/nexstar/images/nexstar.png");
	IconStylePtr icon_style = factory->CreateIconStyle();
	icon_style->set_icon(icon);
	icon_style->set_scale(1.0);
	ua_style->set_iconstyle(icon_style);

	document->add_styleselector(ua_style);

	StylePtr mnr_style = factory->CreateStyle();
	mnr_style->set_id("MNR");
	icon = factory->CreateIconStyleIcon();
	icon->set_href("http://tornadochaser.colorado.edu/images/mnr.png");
	icon_style = factory->CreateIconStyle();
	icon_style->set_icon(icon);
	icon_style->set_scale(1.0);
	mnr_style->set_iconstyle(icon_style);

	document->add_styleselector(mnr_style);
}

ThreadedPipe::commMode DataToKML_Parser::mode() {return ThreadedPipe::DUPLEX;};

/************************************************************\
  |                 DataToKML_Parser::update                   |
  \************************************************************/
/*!
  \brief function called periodically by the thread.
  \pre instance of DataToKML_Parser has been created and initialized
  \post any data in the SAVRx will be parsed, converted to KML and
  output to the inetTxFifo
  */
void DataToKML_Parser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;				// read fd set
	struct timeval timeout;

	FD_ZERO(&readFDs);				// zero read fd set
	FD_ZERO(&writeFDs);				// zero read fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);			// add fifo

	fifo_FDs[OUTBOARD_TX] > fifo_FDs[INBOARD_RX] ? maxFD = fifo_FDs[OUTBOARD_TX] : maxFD = fifo_FDs[INBOARD_RX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("DataToKML_Parser::update() select");
				execRate = -1;
			} else
				cout << "DataToKML_Parser::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(INBOARD_RX);
			if( n < 0 && errno != EAGAIN)
				perror("DataToKML_Parser::update() - error reading from fifo ");
			else {
				if(n>0) {

					uint16_t length = CMD_SIZE(fifo_msg.data);
					if( length != fifo_msg.size) {
						cout << getName() << "::update() - error, length of command != length of buffer" << endl
							<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

						if( length < 1000 && length <= fifo_msg.size ) {
							if( C_calcCheckSum((uint8_t *)buf) == 0 )
								cout << "\tCMD: is valid" << endl;
							else
								cout << "\tCMD: is NOT valid" << endl;
						}

						return;
					}

					Command cmd((uint8_t*)fifo_msg.data);

					if( !cmd.isValid() )
						cout << "DataToKML::update() - error, invalid command in rxfifo"<< endl << cmd ;
					else
						parse(cmd, fifo_msg.src);
				}
			}
		}
	}
}

void DataToKML_Parser::parse(const Command &cmd, in_addr address) 
{
	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);
	Client * a_client = shmPtr->clients.getClient(address);
	if( a_client == NULL ) {
		if( (address.s_addr == inet_addr("10.0.0.5")) ) {
			address.s_addr = inet_addr("6.0.0.16");	
			a_client = shmPtr->clients.getClient(address);
			if( !a_client ) { 
				cout << "sorry, sol" << endl;
				return;
			}
		}

	}

	//cout << "CMD: from=" << inet_ntoa(address) << " cmd=" << DefinedCommands[cmd.cmd()].name << endl;
	switch(cmd.cmd()) {
		case CMD_CAPABILITY: {
			//cout << "CAPABILITY " << a_client->name <<  endl;

			/*if(a_client->telemetry.gps.pos.lat == 0.0) {
			  if(a_client->capabilities.find(CMD_TELEM))
			  subscribe(CMD_TELEM, a_client, true, INBOARD_TX);
			  if(a_client->capabilities.find(CMD_GPS_TELEM))
			  subscribe(CMD_GPS_TELEM, a_client, true, INBOARD_TX);
			 }*/
		} break;
		case CMD_GPS_TELEM: {
			//cout << "GPS TELEMETRY" << endl;
			GPSTelem_t telemetry;
			CopyBufToStruct( dataPtr, telemetry);

			/*CoordinatesPtr coordinates = factory->CreateCoordinates();
			coordinates->add_latlngalt(telemetry.pos.lat,telemetry.pos.lon,telemetry.pos.alt);;
			PointPtr point = factory->CreatePoint();
			point->set_coordinates(coordinates);*/

			LocationPtr location = factory->CreateLocation();
			location->set_longitude(telemetry.pos.lon);
			location->set_latitude(telemetry.pos.lat);
			location->set_altitude(telemetry.pos.alt);

			ModelPtr model = factory->CreateModel();

			model->set_location(location);
			//model->set_altitudemode(kmldom::ALTITUDEMODE_ABSOLUTE);

			OrientationPtr orientation = factory->CreateOrientation();
			orientation->set_heading(fmod((telemetry.track+180),360));
			orientation->set_tilt(0);
			orientation->set_roll(0);
			model->set_orientation(orientation);

			ScalePtr scale = factory->CreateScale();
			scale->set_x(50.0);
			scale->set_y(50.0);
			scale->set_z(50.0);
			model->set_scale(scale);

			LinkPtr link = factory->CreateLink();

			// test for trailer or mnr
			bool isTrailer = false;
			if( a_client->name.find("Trailer") >= 0 ) {
				isTrailer = true;	
				link->set_href("http://tornadochaser.colorado.edu/images/models/trailer.dae");
			} else
				link->set_href("http://tornadochaser.colorado.edu/images/models/mnr.dae");

			model->set_link(link);

			if(placemarks.find(a_client->name.c_str())!=placemarks.end())
			{
				//placemarks[a_client->name.c_str()]->set_geometry(point);
				placemarks[a_client->name.c_str()]->set_geometry(model);
			} else {
				PlacemarkPtr placemark = factory->CreatePlacemark();
				placemark->set_name(a_client->name.c_str());
				placemark->set_styleurl("MNR");
				//placemark->set_geometry(point);
				placemark->set_geometry(model);
				cout << "Adding KML client: " << a_client->name.c_str() << endl;
				placemarks.insert( placemark_hash_t::value_type(a_client->name.c_str(), placemark));
			}

		} break;
		case CMD_TELEM: {
			//cout << "PICCOLO TELEMETRY" << endl;
			Telemetry_t telemetry;
			CopyBufToStruct( dataPtr, telemetry);

			LocationPtr location = factory->CreateLocation();
			location->set_longitude(telemetry.gps.pos.lon);
			location->set_latitude(telemetry.gps.pos.lat);
			location->set_altitude(telemetry.gps.pos.alt + 30);

			ModelPtr model = factory->CreateModel();

			model->set_location(location);
			model->set_altitudemode(kmldom::ALTITUDEMODE_ABSOLUTE);

			OrientationPtr orientation = factory->CreateOrientation();
			//orientation->set_heading(fmod((telemetry.gps.track+180),360));
			//orientation->set_heading(telemetry.gps.track);
			orientation->set_heading(fmod(telemetry.angles.yaw/1000.0*180/M_PI + 180,360));
			orientation->set_tilt(telemetry.angles.pitch/1000.0*180/M_PI);
			orientation->set_roll(telemetry.angles.roll/1000.0*180/M_PI);
			model->set_orientation(orientation);

			ScalePtr scale = factory->CreateScale();
			scale->set_x(5.0);
			scale->set_y(5.0);
			scale->set_z(5.0);
			model->set_scale(scale);

			LinkPtr link = factory->CreateLink();
			link->set_href("http://tornadochaser.colorado.edu/images/models/nexstar.dae");
			model->set_link(link);

			if(placemarks.find(a_client->name.c_str())!=placemarks.end())
			{
				placemarks[a_client->name.c_str()]->set_geometry(model);
			} else {
				PlacemarkPtr placemark = factory->CreatePlacemark();
				placemark->set_name(a_client->name.c_str());
				placemark->set_styleurl("NexSTAR");
				placemark->set_id(a_client->name.c_str());
				placemark->set_geometry(model);
				cout << "Adding KML client: " << a_client->name.c_str() << endl;
				placemarks.insert( placemark_hash_t::value_type(a_client->name.c_str(), placemark));
			}
			break;
		} 
		case CMD_SONDE: {
			break;
		} 
		case CMD_WIND: {
			break;
		} 
		case CMD_FP_SUBS: {
			break;
		} 
		case CMD_FP: {
			break;
		} 
		case CMD_LINK_STATUS: {
			break;
		} 
		case CMD_ACK: {
			break;
		} 
		case CMD_FAIL: {
			break;
		} 
		default:
			break;
	}

	PrintKML();
}

void DataToKML_Parser::PrintKML()
{
	Client * a_client;
	placemark_hash_t::iterator p_iter;
	for ( p_iter = placemarks.begin(); p_iter != placemarks.end(); ) {
		a_client = shmPtr->clients.getClient(String((p_iter->second->get_name()).c_str()));
		if(a_client != NULL) {
			if( a_client->state.timed_out ) {
				cout << a_client->name << " is timed out, removing KML client" << endl;
				if( document->DeleteFeatureById(p_iter->second->get_name()) == NULL )
					cout << "DataToKML_Parser::PrintKML - did not delete feature: " 
					     << p_iter->second->get_name() << endl;
				else
					p_iter = placemarks.erase( p_iter );

				continue;
			} else {
				document->add_feature(p_iter->second);
			}
		}
		p_iter++;
	}

	std::string xml = kmldom::SerializePretty(kml);

	kml_file.open("development.kml",ios::out);
	kml_file << xml << flush;
	kml_file.close();

	//system("cp development.kml /projects/tornado/public_html/production.kml");
	system("cp development.kml /home/elstonj/projects/tornado/public_html/production.kml");
}

DataToKML_Parser::~DataToKML_Parser()
{
}
